#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>
#include <sensor_msgs/Imu.h>

using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=0;
string frame_id_string_,pc_path_name_;
ros::Publisher pose_mean_pub_;


void displayPFPose(const sensor_msgs::Imu::ConstPtr& imuMsg) {
    cout<<"callback ok"<<endl;
    geometry_msgs::PoseStamped p;
    p.header.frame_id = "map";
    p.header.stamp = ros::Time::now();
    p.pose.position.x=0.1;p.pose.position.y=0.1;
    p.pose.position.z=0;
    p.pose.orientation.x=imuMsg->orientation.x;p.pose.orientation.y=imuMsg->orientation.y;
    p.pose.orientation.z=imuMsg->orientation.z;p.pose.orientation.w=imuMsg->orientation.w;
    pose_mean_pub_.publish(p);
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "mcl_3dl");
    ros::NodeHandle nh;
    pose_mean_pub_=nh.advertise<geometry_msgs::PoseStamped>("mean_pose",1);
    ros::Subscriber imusub      = nh.subscribe<sensor_msgs::Imu>  ("nexus0/imu_raw", 1, displayPFPose);
    ros::spin();
    return 0;
}
